/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.grt192.mechanism.breakaway;

import com.grt192.actuator.GRTJaguar;
import com.grt192.core.Command;
import com.grt192.core.Mechanism;
import com.grt192.sensor.GRTMaxBotixSonar;

/**
 *
 * @author Vikram Sunder
 */
public class Roller extends Mechanism {

    public static final double GRAB = -1;
    public static final double PUSH = 1;
    public static final double STOP = 0;
    public static final double CENTER_THRESHOLD = 60;
    public static final double SIDE_THRESHOLD = 80;

    public Roller(GRTJaguar rollerLeft, GRTJaguar rollerRight,
            GRTMaxBotixSonar leftSonar, GRTMaxBotixSonar rightSonar,
            GRTMaxBotixSonar centerSonar) {
        //starts motors
        rollerRight.start();
        rollerLeft.start();

        //adds Actuators
        addActuator("RightRollerMotor", rollerRight);
        addActuator("LeftRollerMotor", rollerLeft);

        //Adds Sensors
        addSensor("LeftSonar", leftSonar);
        addSensor("rightSonar", rightSonar);
        addSensor("centerSonar", centerSonar);
    }

    public void setLeftSpeed(double input) {
        getActuator("LeftRollerMotor").enqueueCommand(new Command(input));
    }

    public void setRightSpeed(double input) {
        getActuator("RightRollerMotor").enqueueCommand(new Command(input));
    }

    public void leftGrab() {
        setLeftSpeed(-GRAB);
    }

    public void rightGrab() {
        setRightSpeed(GRAB);
    }

    public void leftPush() {
        setLeftSpeed(-PUSH);
    }

    public void rightPush() {
        setRightSpeed(PUSH);
    }

    public void stopRightRoller() {
        setRightSpeed(STOP);
    }

    public void stopLeftRoller() {
        setLeftSpeed(STOP);
    }

    public boolean leftObstructed(){
        return leftRange() <= SIDE_THRESHOLD;
    }

    public boolean rightObstructed(){
        return rightRange() <= SIDE_THRESHOLD;
    }

    public boolean centerObstructed(){
        return centerRange() <= CENTER_THRESHOLD;
    }

    public double leftRange(){
        return getSensor("rightSonar").getState("Value");
    }

    public double rightRange(){
        return getSensor("leftSonar").getState("Value");
    }

    public double centerRange(){
        return getSensor("centerSonar").getState("Value");
    }
}
